import static foo.IrSensorUmrechner.irse2cm;
import static foo.IrSensorUmrechner.irsInfinty;
import lejos.nxt.Motor;
import lejos.nxt.addon.EOPD;

public class Ausrichter extends Thread {
	
	private double breiteLinks;
	private double breiteRechts;
	private double gesamtBreite;
	
	private double breitDiffLinks;
	private double breitDiffRechts;
	
	private double abstand;

	private int leftTachoCount;
	private int rightTachoCount;
	private double strecke;
	
	private double lastBreiteLinks;
	private double lastBreiteRechts;
	private double lastLeftTachoCount;
	private double lastRightTachoCount;
	
	private Motor leftMotor;
	private Motor rightMotor;
	
	private EOPD leftIRSensor;
	private EOPD rightIRSensor;
	
	private static boolean activated;
	
	public static final int robotorBreiteCm = 11;
	private static final double radDurchMesserCm = 5.6;
	private double winkel;
	
	private void mainLoop() throws Exception {
		readNewValues();
		Thread.sleep(250);
		
		while (activated && !Thread.interrupted()) {
			updateLastValues();
			readNewValues();
			
			calcNewValues();
			
			while (leftMotor.getTachoCount() - lastLeftTachoCount > 25) {
				Thread.sleep(100);
			}
		}
	}

	public Ausrichter(EOPD leftIRSensor, EOPD rightIRSensor, Motor leftMotor, Motor rightMotor) {
		this.leftIRSensor  = leftIRSensor;
		this.rightIRSensor = rightIRSensor;
		this.leftMotor 	   = leftMotor;
		this.rightMotor    = rightMotor;
		
		activated = true;
	}
	
	@Override
	public void run() {
		try {
			mainLoop();
		} catch(Exception e) {}
	}
	
	private void readNewValues() {
		breiteLinks  = irse2cm(leftIRSensor.readRawValue());
		breiteRechts = irse2cm(rightIRSensor.readRawValue());
		
		
		leftTachoCount  = leftMotor.getTachoCount();
		rightTachoCount = rightMotor.getTachoCount();
	}
	
	private void updateLastValues() {
		lastBreiteLinks  = breiteLinks;
		lastBreiteRechts = breiteRechts;
		
		lastLeftTachoCount  = leftTachoCount;
		lastRightTachoCount = rightTachoCount;
	}
	
	private void calcNewValues() {
		breitDiffLinks  = lastBreiteLinks - breiteLinks;
		breitDiffRechts = lastBreiteRechts - breiteRechts;
		
		if 	((Math.abs(leftMotor.getSpeed() - rightMotor.getSpeed()) < 10) &&
			(leftMotor.getMode() == rightMotor.getMode())){
			
			winkel = 0.0;
			return;
		}
		
		strecke = ((lastLeftTachoCount - leftTachoCount) +
				  (lastRightTachoCount - rightTachoCount))  / 360.0 / 2.0 *
				  (radDurchMesserCm / 2.0 * Math.PI);
		
		if (lastBreiteLinks < irsInfinty && breiteLinks < irsInfinty )
			winkel = Math.atan(breitDiffLinks/strecke);
		else if (lastBreiteRechts < irsInfinty && breiteRechts < irsInfinty ) {
			winkel = Math.atan(breitDiffRechts/strecke);
		} else {
			winkel = 0.001;
		}
		
		if (breiteLinks < irsInfinty && breiteRechts < irsInfinty) 
			gesamtBreite = breiteLinks + breiteRechts + robotorBreiteCm;
		
		if (breiteLinks < irsInfinty && breiteRechts < irsInfinty) {
			abstand = Math.min(breiteLinks, breiteRechts);
		} else if (breiteLinks < irsInfinty) {
			abstand = breiteLinks;
		} else if (breiteRechts < irsInfinty) {
			abstand = breiteRechts;
		} 
	}
	
	public double getWinkel() {
		return winkel;
	}
	
	public double getBreiteDiff() {
		return breitDiffLinks;
	}
	
	public double getAbstand() {
		return abstand;
	}
	
	public double getBreite() {
		return gesamtBreite;
	}
}